
#include "cmsis_os.h"
#include "pid.h"
#include "remote_control.h"
#include "NUC_communicate.h"
#include "robot_total_mode_task.h"
#include "CAN_receive.h"
#include "main.h"
#include "detect_task.h"
#include "OLED.h"
#include "referee.h"
#include "robot_config.h"
#include "struct_typedef.h"
#include "debug_config.h"
#ifdef GIMBAL_WITHOUT_IMU

// 注意：当遥控器右拨杆指向中央的时候，才允许鼠标操控

//*************任务设置
#define SHOOT_TASK_INIT_TIME 500
#define SHOOT_CTRL_TIME 5

#define M3508_MOTOR_SPEED_PID_KP 1.0f
#define M3508_MOTOR_SPEED_PID_KI 0.0f
#define M3508_MOTOR_SPEED_PID_KD 0.0f

#define M2006_MOTOR_SPEED_PID_KP 1.6f
#define M2006_MOTOR_SPEED_PID_KI 0.0f // 不要这个积分项，否则连续发射停止后还会转
#define M2006_MOTOR_SPEED_PID_KD 0.0f

#define MAX_MOTOR_CAN_CURRENT 15000.0f
#define M3508_MOTOR_SPEED_PID_MAX_OUT MAX_MOTOR_CAN_CURRENT
#define M3508_MOTOR_SPEED_PID_MAX_IOUT 2000.0f

#define MAX_MOTOR_CAN_CURRENT_M2006 10000.0f
#define M2006_MOTOR_SPEED_PID_MAX_OUT MAX_MOTOR_CAN_CURRENT_M2006
#define M2006_MOTOR_SPEED_PID_MAX_IOUT 2000.0f

//*************************里程碑栈法的控制参数***********//
#define MILESTONE_NEAR_THRESHHOLD 2000
#define MILESTONE_NUMBER 3

//**********************卡弹相关***********************//
#define STUCK_TIME_LIMIT 800   // 当在发射一发的模式中持续一段时间不退出后，进入卡弹解决模式  //500
#define SOLVING_TIME_LIMIT 200 // 在尝试卡弹解决模式中停留的时间。 //300

/********************单发圈数控制相关*******************/
#define TRIGGER_RANGE 0.5
#define TRIGGER_ROUNDS 7.5f

#define WITHOUT_HEAT_DETECT 2

const static fp32 motor_speed_pid[3] = {M3508_MOTOR_SPEED_PID_KP, M3508_MOTOR_SPEED_PID_KI, M3508_MOTOR_SPEED_PID_KD};
const static fp32 trigger_motor_speed_pid[3] = {M2006_MOTOR_SPEED_PID_KP, M2006_MOTOR_SPEED_PID_KI, M2006_MOTOR_SPEED_PID_KD};

enum TriggerMode_e
{
    TRIGGERMODE_E_STOP,       // 停止发射
    TRIGGERMODE_E_SHOOTONE,   // 发射一个，靠拨弹轮减速前的电机转4圈。对应弹盘轮子转一格
    TRIGGERMODE_E_SHOOTMULTI, // 多发，其实是等待进入发射一个的状态
    TRIGGERMODE_E_STUCKSOLVE, // 尝试解决卡弹
};

struct milestoneStack_t
{
    uint8_t head;
    uint8_t stack[MILESTONE_NUMBER + 1];
};

struct TriggerControl_t
{
    struct milestoneStack_t mstack; // 里程碑栈
    const uint16_t *ECDPoint;       // 电机ECD所在位置
    uint16_t initECD;               // 初始ECD
    uint16_t nowECD;                // 现在的ECD
    uint16_t lastEcd;
    int16_t stepEcd;
    fp32 nowRounds;              // 现在转过的圈数
    fp32 lastRounds;              // 上一次转过的圈数
    fp32 stepRounds;              // 这发转过的圈数
};

//**********缓冲指令
struct InstructBuff_t
{
    uint8_t turnOnFric; // 为1时表示指令开启摩擦轮，为0时表示指令关闭摩擦轮
    uint8_t shootOne;
    uint8_t shootMulti;
};

//*******************全局变量********//
// 摩擦轮开启
static uint8_t fricOnUp = 0;

// 拨弹轮开启
static uint8_t triggerOnUp = 0;
static uint8_t reverseUp = 0;

static struct InstructBuff_t insBuffUp; // 全局发射机构控制指令缓冲
// 在云台任务中控制速度
static int zeroCurrentMarkUp; // 当离线时，直接发送零电流的时候
const static RC_ctrl_t *rc_pUp;
static const fromNUC_t *nuc_pUp; // NUC数据位置。未来制造全自动机器人时需要
static const remote_control_t* remoteControl_pUp ;//图传链路控制
static int16_t giveShootCurrentUp[2];
static int16_t giveTriggerCurrentUp;
static pid_type_def shootMotorPIDsUp[2], triggerMotorPIDUp;
static fp32 wantedVShootMotorUp[2], wantedVTriggerMotorUp;
// int16_t motor_speed_rpm;
static fp32 presentVShootMotorUp[2], presentVTriggerMotorUp;
static struct TriggerControl_t triggerCtrlUp;
static enum TriggerMode_e triggerModeUp;
static uint8_t *robotModeUp;
static uint16_t heatlimitUp;
static uint16_t heatUp;
static uint8_t lastHasBeenShoot=1;//弹丸不是瞬发,1代表前面一发弹丸已发射状态；
static uint16_t lastHeat=0;
static uint8_t numsOfBeenShoot=0;

static void init_shoot_PIDs(void)
{
    uint8_t i;
    for (i = 0; i < 2; i++)
        PID_init(&shootMotorPIDsUp[i], PID_POSITION, motor_speed_pid, M3508_MOTOR_SPEED_PID_MAX_OUT, M3508_MOTOR_SPEED_PID_MAX_IOUT);
    PID_init(&triggerMotorPIDUp, PID_POSITION, trigger_motor_speed_pid, M2006_MOTOR_SPEED_PID_MAX_OUT, M2006_MOTOR_SPEED_PID_MAX_IOUT);
}

static void init_shoot_modes(void)
{
    // 初始化指令
    insBuffUp.shootMulti = 0;
    insBuffUp.shootOne = 0;
    insBuffUp.turnOnFric = 0;
    triggerModeUp = TRIGGERMODE_E_STOP;
    fricOnUp = 0;
    triggerOnUp = 0;
}

static void check_NUC_offLine_and_set_insBuf(void)
{
    if (*robotModeUp == ROBOT_STATE_AUTO)
    {
        if (toe_is_error(MINI_PC_TOE))
        {
            insBuffUp.turnOnFric = 0;
            insBuffUp.shootMulti = 0;
            insBuffUp.shootOne = 0;
            zeroCurrentMarkUp = 1;
        }
    }
    else if (*robotModeUp == ROBOT_STATE_POWERLESS)
    {
        zeroCurrentMarkUp = 1;
    }
}

// static uint16_t ECD_format(int16_t rawECD)
// {
//    while (rawECD < 0)
//        rawECD += ECD_FULL_ROUND;
//    while (rawECD >= ECD_FULL_ROUND)
//        rawECD -= ECD_FULL_ROUND;
//    return (uint16_t)rawECD;
// }

static void init_trigger_ECD_rounds_monitor() // 初始化拨弹轮圈数监控
{
    triggerCtrlUp.ECDPoint = &(get_motor_measure_point(SHOOT_UP_TRIGGER)->ecd);
    triggerCtrlUp.initECD = *(triggerCtrlUp.ECDPoint);
    triggerCtrlUp.nowRounds=0;
    triggerCtrlUp.lastEcd = *(triggerCtrlUp.ECDPoint);
    triggerCtrlUp.stepEcd=0;
    triggerCtrlUp.lastRounds=0;
    triggerCtrlUp.stepRounds=0;
}

static void get_instruction_and_buff(void)
{
    static uint32_t pressTime = 0;
    static uint8_t up = 0, down = 0;
    zeroCurrentMarkUp = 0;
    if (toe_is_error(DBUS_TOE))
    {
        zeroCurrentMarkUp = 1;
        triggerOnUp = 0;
        fricOnUp = 0;
        triggerModeUp = TRIGGERMODE_E_STOP;
        insBuffUp.turnOnFric = 0;
        insBuffUp.shootOne = 0;
        insBuffUp.shootMulti = 0;
    }

    if (*robotModeUp == ROBOT_STATE_AUTO)
    {
        if(fricOnUp==0){
            osDelay(1000);
            insBuffUp.turnOnFric = 1;
        }
        if (shoot_NUC_is_available())
        {
            if (nuc_pUp->shootUp == 1)
            {
                insBuffUp.turnOnFric = 1;
                insBuffUp.shootOne = 1;
            }
            shoot_clear_read_mark();
        }
    }
    else
    {
        if(switch_is_up(rc_pUp->rc.ch[RC_CHOOSE_GIMBAL])){
            if (switch_is_up(rc_pUp->rc.ch[RC_CONTROL_SHOOT]))
            {
                up = 1;
            }
            else if (switch_is_mid(rc_pUp->rc.ch[RC_CONTROL_SHOOT]))
            {
                // 通过上拨和下拨，实现摩擦轮状态的控制
                if (up == 1) // 从上到中 下拨
                {
                    if (fricOnUp)
                    {
                        // fricOnUp=0;
                        // triggerOnUp=0;
                        // 为了防止卡弹，使用缓冲指令设计。当拨弹轮停止时才能关闭摩擦轮。
                        insBuffUp.shootMulti = 0;
                        insBuffUp.shootOne = 0;
                        insBuffUp.turnOnFric = 0; // 希望关闭摩擦轮
                    }
                    else
                    {
                        // fricOnUp=1;
                        insBuffUp.turnOnFric = 1; // 希望开启摩擦轮
                    }
                    up = 0;
                }
                if (down == 1) // 刚刚是下方，从下到中的上拨
                {
                    insBuffUp.shootMulti = 0;          // 停止连发
                    if (pressTime < KEEP_SHOOT_TIME) // 拨下并很快抬起，发射一颗
                        if (pressTime > 0)
                        {
                            insBuffUp.shootOne = 1;
                        }
                    pressTime = 0;
                    down = 0;
                    insBuffUp.shootMulti = 0;
                }
            }
            else if (switch_is_down(rc_pUp->rc.ch[RC_CONTROL_SHOOT]))
            {
                down = 1;
                if (pressTime < KEEP_SHOOT_TIME)
                    pressTime += SHOOT_CTRL_TIME;
                else
                {
                    insBuffUp.shootMulti = 1;
                }
            }
        }
    }
}

static void heat_detect(void)
{
    #ifdef SENTRY
    if(!game_not_started){
        get_shoot_heat_limit_and_heat_sentry_2(&heatlimitUp, &heatUp);
        if(!lastHasBeenShoot&&lastHeat>heatUp){
            lastHasBeenShoot=1;
            if(numsOfBeenShoot<=WITHOUT_HEAT_DETECT){
                numsOfBeenShoot++;
            }
        }
        if (heatUp >= (heatlimitUp - 40)||(numsOfBeenShoot>WITHOUT_HEAT_DETECT&&!lastHasBeenShoot))
        {
            insBuffUp.shootMulti = 0;
            insBuffUp.shootOne = 0;
        }
    }else{
        get_shoot_heat_limit_and_heat_sentry_2(&heatlimitUp, &heatUp);
        if (heatUp >= (heatlimitUp - 40))
        {
            insBuffUp.shootMulti = 0;
            insBuffUp.shootOne = 0;
        }
    } 
    #endif
    if(insBuffUp.shootOne||insBuffUp.shootMulti){
        lastHasBeenShoot=0;
    }
    lastHeat=heatUp;
}

static void fric_mode_change(void)
{
    // 等待拨弹轮关闭时再关闭摩擦轮
    if ((!(insBuffUp.turnOnFric)) || (*robotModeUp) == ROBOT_STATE_POWERLESS)
    {
        if (triggerModeUp == TRIGGERMODE_E_STOP)
        {
            fricOnUp = 0;
            insBuffUp.shootMulti = 0;
            insBuffUp.shootOne = 0;
            insBuffUp.turnOnFric = 0;
        }
    }
    else
    {
        fricOnUp = 1;
    }
}

static void trigger_mode_change(void)
{
    static uint32_t lastShootMultiTime = 0;
    static uint32_t shootOneEnterTime = 0;
    static uint32_t solveStuckEnterTime = 0; // 在发射一发的状态中停留的时间
    static uint32_t nowTime = 0;
    nowTime = HAL_GetTick();
    if (!insBuffUp.turnOnFric) // 摩擦轮关闭状态，始终让拨弹轮关闭
    {
        triggerModeUp = TRIGGERMODE_E_STOP;
        return;
    }
    // 对每个状态枚举
    if (triggerModeUp == TRIGGERMODE_E_STOP)
    {
        // 机器人不处于无力状态时，接收从stop状态转变为其他状态的指令
        if ((*robotModeUp) != ROBOT_STATE_POWERLESS)
        {
            if (insBuffUp.shootOne)
            {
                triggerModeUp = TRIGGERMODE_E_SHOOTONE;
                shootOneEnterTime = nowTime;
                insBuffUp.shootOne = 0; // 清除缓冲的指令
            }
            else if (insBuffUp.shootMulti)
            {
                triggerModeUp = TRIGGERMODE_E_SHOOTMULTI;
                insBuffUp.shootMulti = 0; // 清除缓冲的指令
            }
        }
    }
    else if (triggerModeUp == TRIGGERMODE_E_SHOOTONE)
    {
        triggerCtrlUp.stepRounds=triggerCtrlUp.nowRounds-triggerCtrlUp.lastRounds;
        if (triggerCtrlUp.stepRounds>= TRIGGER_ROUNDS||triggerCtrlUp.stepRounds<=-TRIGGER_ROUNDS)
        {
            triggerModeUp = TRIGGERMODE_E_STOP;  
            triggerCtrlUp.lastRounds=triggerCtrlUp.nowRounds;
        }
        else if (nowTime - shootOneEnterTime > STUCK_TIME_LIMIT)
        {
            triggerModeUp = TRIGGERMODE_E_STUCKSOLVE; // 到达了卡弹的临界
            solveStuckEnterTime = nowTime;
        }
        // 否则留在这个状态
    }
    else if (triggerModeUp == TRIGGERMODE_E_SHOOTMULTI)
    {
        if ((nowTime - lastShootMultiTime) >= SHOOT_MULTI_TIME_GAP)
        {
            lastShootMultiTime = nowTime;
            triggerModeUp = TRIGGERMODE_E_SHOOTONE;
            shootOneEnterTime = nowTime;
        }
    }
    else if (triggerModeUp == TRIGGERMODE_E_STUCKSOLVE)
    {
        if ((nowTime - solveStuckEnterTime) >= SOLVING_TIME_LIMIT)
        {
            triggerModeUp = TRIGGERMODE_E_STOP;
        }
    }
}

static void detect_fric_offLine(void)
{
#ifdef SENTRY
    if ((toe_is_error(SHOOT_DOWN_FRIC_L_TOE)) || (toe_is_error(SHOOT_DOWN_FRIC_R_TOE)))
    {
        fricOnUp = 0;
        triggerOnUp = 0;
        insBuffUp.shootMulti = 0;
        insBuffUp.shootOne = 0;
        insBuffUp.turnOnFric = 0;
    }
#else
    if ((toe_is_error(SHOOT_FRIC_L_TOE)) || (toe_is_error(SHOOT_FRIC_R_TOE)))
    {
        fricOnUp = 0;
        triggerOnUp = 0;
        insBuffUp.shootMulti = 0;
        insBuffUp.shootOne = 0;
        insBuffUp.turnOnFric = 0;
    }
#endif
}

static void set_speed_by_mode(void)
{
    if (fricOnUp)
    {
        wantedVShootMotorUp[0] = -SHOOT_SPEED_LIMIT; // 实验测试转动方向。反过来可以实验卡弹时的退弹
        wantedVShootMotorUp[1] = SHOOT_SPEED_LIMIT;
    }
    else
    {
        wantedVShootMotorUp[0] = 0;
        wantedVShootMotorUp[1] = 0;
    }
    if (triggerModeUp == TRIGGERMODE_E_SHOOTONE || triggerModeUp == TRIGGERMODE_E_STUCKSOLVE)
        triggerOnUp = 1;
    else
        triggerOnUp = 0;

    reverseUp = 0;
    if (triggerModeUp == TRIGGERMODE_E_STUCKSOLVE)
    {
        reverseUp = 1;
    }

    if (triggerOnUp){
        #ifdef INFANTRY
        wantedVTriggerMotorUp = reverseUp ? (0) : (-SHOOT_TRIGGER_SPEED_LIMIT_UP); // 若正在解决stuck(reverseUp)，则以逆向v/4的速度转动
        #endif
        #ifdef HERO
        wantedVTriggerMotorUp = reverseUp ? (0) : (SHOOT_TRIGGER_SPEED_LIMIT_UP);
        #endif
        #ifdef SENTRY
        wantedVTriggerMotorUp = reverseUp ? (0) : (SHOOT_TRIGGER_SPEED_LIMIT_UP);
        #endif
    }else{
        wantedVTriggerMotorUp = 0;
    }
}

static void calc_give_current(void)
{
    int i;
    for (i = 0; i < 2; i++)
        PID_calc(&shootMotorPIDsUp[i], presentVShootMotorUp[i], wantedVShootMotorUp[i]);
    for (i = 0; i < 2; i++)
        giveShootCurrentUp[i] = shootMotorPIDsUp[i].out;
    PID_calc(&triggerMotorPIDUp, presentVTriggerMotorUp, wantedVTriggerMotorUp);
    giveTriggerCurrentUp = triggerMotorPIDUp.out;
}

static void get_shoot_motor_speed(void)
{
    uint8_t i;
    for (i = 0; i < 2; i++)
    {
        presentVShootMotorUp[i] = get_motor_measure_point(i + SHOOT_UP_FRIC_L)->speed_rpm;
    }
    presentVTriggerMotorUp = get_motor_measure_point(SHOOT_UP_TRIGGER)->speed_rpm;
}

void shoot_up_task(void const *pvParameters)
{
    osDelay(SHOOT_TASK_INIT_TIME);
    init_shoot_PIDs();                 // 初始化PID
    init_shoot_modes();                // 初始化发射状态
    robotModeUp = get_robot_present_mode();
    rc_pUp = get_remote_control_point(); // 获取遥控器数据和NUC数据指针
    nuc_pUp = get_nuc_control_point();
    remoteControl_pUp=get_usart_remote_control_p();
    while (1)
    {
        get_instruction_and_buff(); // 获取指令并缓冲
        check_NUC_offLine_and_set_insBuf();
        detect_fric_offLine();
#ifdef WITH_REFEREE
        heat_detect();
#endif
        fric_mode_change();    // 摩擦轮状态改变
        trigger_mode_change(); // 拨弹轮状态转变
        set_speed_by_mode();
        get_shoot_motor_speed();
        calc_give_current(); // 计算PID
#ifdef ZERO_CURRENT_SAFE_SHOOT_UP
        zeroCurrentMarkUp = 1;
#endif
        if (zeroCurrentMarkUp)
        {
            giveTriggerCurrentUp=0;
            giveShootCurrentUp[0]=0;
            giveShootCurrentUp[1]=0;
        }
        osDelay(SHOOT_CTRL_TIME);
    }
}

const int16_t* get_trigger_up_current_p(void){
    return &giveTriggerCurrentUp;
}

const int16_t* get_fric_up_current_p(void)
{
    return giveShootCurrentUp;
}

static void monitor_trigger_rounds(void){
    triggerCtrlUp.nowECD = *(triggerCtrlUp.ECDPoint);
    triggerCtrlUp.stepEcd=triggerCtrlUp.nowECD-triggerCtrlUp.lastEcd;
    while (triggerCtrlUp.stepEcd<-ECD_FULL_ROUND/2){//按设定转速不可能超过
        triggerCtrlUp.stepEcd+=ECD_FULL_ROUND;
    }
    while (triggerCtrlUp.stepEcd>ECD_FULL_ROUND/2){
        triggerCtrlUp.stepEcd-=ECD_FULL_ROUND;
    }
    triggerCtrlUp.nowRounds+=(fp32)triggerCtrlUp.stepEcd/ECD_FULL_ROUND;
    if(triggerCtrlUp.nowRounds>TRIGGER_ROUNDS*1000){
        triggerCtrlUp.nowRounds-=TRIGGER_ROUNDS*1000;
    }
    if(triggerCtrlUp.nowRounds<-TRIGGER_ROUNDS*1000){
        triggerCtrlUp.nowRounds+=TRIGGER_ROUNDS*1000;
    }
    triggerCtrlUp.lastEcd=triggerCtrlUp.nowECD;
}

void shoot_up_trigger_monitor(void const *pvParameters)
{
    osDelay(SHOOT_TASK_INIT_TIME);
    init_trigger_ECD_rounds_monitor(); // 初始化拨弹轮圈数监控
    while (1)
    {
        monitor_trigger_rounds();
        osDelay(1);
    }
}

#endif
